/* Functions for Marquardt-Levenberg minimization routine.
   All functions are of the form y = F(x, p, dydp, npar). */

/* By:   S.C. Molitor (smolitor@bme.jhu.edu)
   Date: February 22, 1999 */


// inclusions

#include "mex.h"
#include <math.h>


// BOLTZMANN - sum of Boltzmann functions with offset & scaling
//             sum is 1 / (1 + Kn + Kn * Kn-1 + Kn * Kn-1 * Kn-2 + ...)
//             each Boltzmann has a slope & xhalf:  Ki = exp(-ki * (x - xi))
//             first two overall parameters are Amin & Amax
//             must have 2N + 2 parameters for N Boltzmanns

double oneboltzmann(double	x,		// x value, returns f(p, x)
				 double	p[],	// parameter values in p
				 double	dfdp[],	// partial derivatives df/dp
				 int	npar) {	// number of parameters in p

	// declare variables
	
	double	product;	// product of Boltzmanns
	double	expfac;		// exponential calculation
	double	*dfdpfac;	// storage for df/dp scaling
	double	y;			// evaluated f(p, x)
	int		nboltz;		// number of Boltzmanns added
	int		i, j;		// loop counters

	// calculate number of Boltzmanns added together
	// check number of parameters
	// if error, return zero for f(p, x) & df/dp

	nboltz = (npar - 2) / 2;
	if (npar != (2 * nboltz + 2)) {
		for (i = 0; i < npar; i++) {
			dfdp[i] = 0.0;
		}
		return(0.0);
	}

	// allocate space for df/dp calculations

	dfdpfac = (double *) mxCalloc(nboltz, sizeof(double));

	// calculate individual Boltzmann values
	// calculate sums & product of Boltzmanns

	for (product = 1.0, i = nboltz; i >= 1; i--) {
		expfac = exp(-p[2 * i] * (x - p[2 * i + 1]));
		product *= expfac;
		for (j = i - 1; j < nboltz; j++) {
			dfdpfac[j] += product;
		}
	}

	// complete calculations for f(p, x) and df/dp
	// free allocated space
	// return f(p, x)

	dfdp[1] = 1.0 / (1.0 + dfdpfac[nboltz - 1]);
	dfdp[0] = 1.0 - dfdp[1];
	y = p[0] + (p[1] - p[0]) * dfdp[1];
	expfac = p[1] * dfdp[1] * dfdp[1];
	for (i = 1; i <= nboltz; i++) {
		dfdp[2 * i] = expfac * dfdpfac[i - 1] * (x - p[2 * i + 1]);
		dfdp[2 * i + 1] = expfac * dfdpfac[i - 1] * (-p[2 * i]);
	}
	mxFree((void *) dfdpfac);
	return(y);
}

// SUM_BOLTZMANN - sum of independent Boltzmann functions with offset & scaling
//             each Boltzmann has an amplitude, a slope & xhalf:  Ki = Ai*exp(-ki * (x - xi))
//             first parameters is an offset
//             must have 3N + 1 parameters for N Boltzmanns
//			   in order of [A0 A1 K1 X1 A2 K2 X2....]
//		Contributed by P. Manis 1/21/2000

double sumboltzmann(double	x,		// x value, returns f(p, x)
				 double	p[],	// parameter values in p
				 double	dfdp[],	// partial derivatives df/dp
				 int	npar) {	// number of parameters in p

	// declare variables
	
	double	expfac;		// exponential calculation
	double	*dfdpfac;	// storage for df/dp scaling
	double	y;			// evaluated f(p, x)
	int		nboltz;		// number of Boltzmanns added
	int		i, j;		// loop counters
	double ai, ki, xi;	// temp variables for a, k and x each function
	int ind_a, ind_k, ind_x; // indicex...
	// calculate number of Boltzmanns added together
	// check number of parameters
	// if error, return zero for f(p, x) & df/dp
#define NPAR 3

	nboltz = (npar - 1) / NPAR;
	if (npar != (NPAR * nboltz + 1)) {
		for (i = 0; i < npar; i++) {
			dfdp[i] = 0.0;
		}
		return(0.0);
	}

	// allocate space for df/dp calculations

	dfdpfac = (double *) mxCalloc(nboltz, sizeof(double));

	
	// complete calculations for f(p, x) and df/dp
	// free allocated space
	// return f(p, x)
    // A0 is p[0]
    // Ai is p[3*i-2] (p[1] is A1 (i=1))
    // Ki is p[3*i-1] (p[2] is K1 (i=1))
    // Xi is p[3*i] (p[3] is X1 (i=1))
    
	dfdp[0] = 1.0;
	y = p[0];
	for (i = 1; i <= nboltz; i++) {
		ind_a = NPAR * i - (NPAR-1);
		ind_k = NPAR * i - (NPAR-2);
		ind_x = NPAR * i;
		ai = p[ind_a];
		xi = p[ind_x];
		ki = p[ind_k];
		expfac = exp((-x + xi) * ki);
		dfdp[ind_a] = 1/(1 + expfac);
		dfdp[ind_x] = -ai * ki * expfac * (dfdp[ind_a]*dfdp[ind_a]);
		dfdp[ind_k] = dfdp[ind_x] * (-x + xi) / ki;
		y = y + ai / (1 + expfac);
	}
	mxFree((void *) dfdpfac);
	return(y);
}

// CURR_BOLTZMANN - sum of independent Boltzmann functions translated to current space
//             with offset & scaling
//             each Boltzmann has an amplitude, a slope & xhalf and Vr:  
//		Y = Ai*(V-Vri)/(1+exp(-ki * (x - xi)))
//             first parameter is an offset
//             must have 4N + 1 parameters for N Boltzmanns
//			   in order of [A0 A1 K1 X1 V1 A2 K2 X2 V2....]
//		Contributed by P. Manis 1/21/2000

double currboltzmann(double	x,		// x value, returns f(p, x)
				 double	p[],	// parameter values in p
				 double	dfdp[],	// partial derivatives df/dp
				 int	npar) {	// number of parameters in p

	// declare variables
	
	double	expfac, expfac1;		// exponential calculation
	double	*dfdpfac;	// storage for df/dp scaling
	double	y;			// evaluated f(p, x)
	int		nboltz;		// number of Boltzmanns added
	int		i, j;		// loop counters
	double ai, ki, xi, vri;	// temp variables for a, k, x and vr each function
	int ind_a, ind_k, ind_x, ind_vr; // indices...
	// calculate number of Boltzmanns added together
	// check number of parameters
	// if error, return zero for f(p, x) & df/dp
#define NPR 4

	nboltz = (npar - 1) / NPR;
	if (npar != (NPR * nboltz + 1)) {
		for (i = 0; i < npar; i++) {
			dfdp[i] = 0.0;
		}
		return(0.0);
	}

	// allocate space for df/dp calculations

	dfdpfac = (double *) mxCalloc(nboltz, sizeof(double));

	
	// complete calculations for f(p, x) and df/dp
	// free allocated space
	// return f(p, x)
    // A0 is p[0]
    // Ai is p[4*i-3] (p[1] is A1 (i=1))
    // Ki is p[4*i-2] (p[2] is K1 (i=1))
    // Xi is p[4*i-1] (p[3] is X1 (i=1))
    // Vri is p[4*i] (p[4] is vri1 (i=1))
        
	dfdp[0] = 1.0;
	y = p[0];
	for (i = 1; i <= nboltz; i++) {
		ind_a = NPR * i - (NPR-1);
		ind_k = NPR * i - (NPR-2);
		ind_x = NPR * i - (NPR-3);
		ind_vr = NPR * i;
		ai = p[ind_a];
		xi = p[ind_x];
		ki = p[ind_k];
		vri = p[ind_vr];
		expfac = exp((-x + xi) * ki);
		expfac1 = 1/(1+expfac);
		dfdp[ind_a] = (x - vri)*expfac1;
		dfdp[ind_x] = - ai * dfdp[ind_a] * expfac1 * expfac * ki;
		dfdp[ind_k] = - ai * dfdp[ind_a] * expfac1 * expfac * (-x + xi);
		dfdp[ind_vr] = - ai*expfac1;
		y = y + ai * dfdp[ind_a];
	}
	mxFree((void *) dfdpfac);
	return(y);
}


// EXPONENTIAL - sum of exponential functions with offset
//               each exponential must have an amplitude & time constant
//               first overall parameter is offset
//               must have 2N + 1 parameters for N exponentials

double exponential(double	x,		// x value, returns f(p, x)
				   double	p[],	// parameter values in p
				   double	dfdp[],	// partial derivatives df/dp
				   int		npar) {	// number of parameters in p

	// declare variables
	
	bool	errflag;	// error flag
	double	y, yi;		// evaluated f(p, x)
	int		nexp;		// number of exponentials added
	int		i;			// loop counter

	// calculate number of exponentials added together
	// check number of parameters
	// check for positive time constants

	errflag = false;
	nexp = (npar - 1) / 2;
	if (npar == (2 * nexp + 1)) {
		for (i = 1; i <= nexp; i++) {
			if (p[2 * i] <= 0.0) {
				errflag = true;
				break;
			}
		}
	}
	else {
		errflag = true;
	}

	// if error, return zero for f(p, x) & df/dp

	if (errflag) {
		for (i = 0; i < npar; i++) {
			dfdp[i] = 0.0;
		}
		return(0.0);
	}

	// calculate df/dp
	// sum individual exponentials

	y = p[0];
	dfdp[0] = 1.0;
	for (i = 1; i <= nexp; i++) {
		dfdp[2 * i - 1] = exp(-x / p[2 * i]);
		yi = p[2 * i - 1] * dfdp[2 * i - 1];
		dfdp[2 * i] = yi * x / (p[2 * i] * p[2 * i]);
		y += yi;
	}
	return(y);
}


// GAUSSIAN - sum of Gaussian PDF functions with offset & scaling
//            each Gaussian must have an amplitude, mean & std. dev.
//            first overall parameter is offset
//            must have 3N + 1 parameters for N Gaussians

double gaussian(double	x,		// x value, returns f(p, x)
				double	p[],	// parameter values in p
				double	dfdp[],	// partial derivatives df/dp
				int		npar) {	// number of parameters in p

	// declare variables
	
	bool	errflag;	// error flag
	double	ampfac;		// amplitude scaling
	double	normx;		// x normalized by mu & omega
	double	expfac;		// exponential factor
	double	y, yi;		// evaluated f(p, x)
	int		npdf;		// number of gaussian PDFs added
	int		i;			// loop counter

	// calculate number of Gaussian PDFs added together
	// check number of parameters
	// check for positive std. deviations

	errflag = false;
	npdf = (npar - 1) / 3;
	if (npar == (3 * npdf + 1)) {
		for (i = 1; i <= npdf; i++) {
			if (p[3 * i] <= 0.0) {
				errflag = true;
				break;
			}
		}
	}
	else {
		errflag = true;
	}

	// if error, return zero for f(p, x) & df/dp

	if (errflag) {
		for (i = 0; i < npar; i++) {
			dfdp[i] = 0.0;
		}
		return(0.0);
	}

	// calculate df/dp
	// sum individual Gaussian PDFs

	y = p[0];
	dfdp[0] = 1.0;
	for (i = 1; i <= npdf; i++) {
		ampfac = 1.0 / (sqrt(8.0 * atan2(1, 1)) * p[3 * i]);
		normx = (x - p[3 * i - 1]) / p[3 * i];
		expfac = exp(-0.5 * normx * normx);
		dfdp[3 * i - 2] = ampfac * expfac;
		yi = p[3 * i - 2] * dfdp[3 * i - 2];
		dfdp[3 * i - 1] = yi * normx / p[3 * i];
		dfdp[3 * i] = yi * (normx * normx - 1.0) / p[3 * i];
		y += yi;
	}
	return(y);
}
